/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera.*;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class orangebot extends IterativeRobot {

    Joystick joyRight = new Joystick (1);                                      // Right Joystick is in Port 1
    Joystick joyLeft = new Joystick (2);
    Gyro Orange = new Gyro(2);
    DigitalInput leftline = new DigitalInput (1);
    DigitalInput centerline = new DigitalInput (3);
    DigitalInput rightline = new DigitalInput (5);
    RobotDrive OrangeDrive = new RobotDrive (1,3,2,4);
    int GyroCount = 1;                                                         // The first variable for the Gyro Counter
    AxisCamera camera;
    ColorImage image;
    Servo zCam = new Servo (7);
    Relay cameralight = new Relay(1);
    Victor forklift1 = new Victor(5);
    Victor forklift2 = new Victor(6);
    Solenoid minibot = new Solenoid(1);

    public void robotInit() {

       System.out.println("ROBOT INIT IS GETTING CALLED!");
       Orange.reset();
       OrangeDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
       OrangeDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
       OrangeDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
       camera = AxisCamera.getInstance();
       camera.writeResolution(AxisCamera.ResolutionT.k320x240);
       camera.writeBrightness(25);
       camera.writeCompression(0);

    }

    public void autonomousPeriodic() {

    }

    public void teleopPeriodic() {

        if (joyRight.getRawButton(7))
        {
            Orange.reset();
        }
        minibot.set(joyLeft.getTrigger() && joyRight.getTrigger());
        zCam.set(((joyLeft.getThrottle()* -1) + 1) / 2);
        cameralight.setDirection(Relay.Direction.kForward);
        cameralight.set(Relay.Value.kOn);
        getlinetrackermovement();
        CameraImage();
        updateDashboard();
        ForkLift();

    }

    public void getlinetrackermovement() {
    int left = leftline.get()?(int)100:(int)0;
    int center = centerline.get()?(int)10:(int)0;
    int right = rightline.get()?(int)1:(int)0;
    int linetotal = left + center + right;

    switch(linetotal){
        default:
            OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle()+180);
            break;
        case 111:
            System.out.println("Center");
            OrangeDrive.mecanumDrive_Cartesian(0,.3 ,0, Orange.getAngle());
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
        case 100:
            System.out.println("Right");
            OrangeDrive.mecanumDrive_Cartesian(-.3,0 ,0, Orange.getAngle());
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
        case 001:
            System.out.println("Left");
            OrangeDrive.mecanumDrive_Cartesian(.3,0 ,0, Orange.getAngle());
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
        case 101:
            System.out.println("No Lines");
            OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            break;
        case 010:
            System.out.println("All Lines");
            OrangeDrive.mecanumDrive_Cartesian(0 ,0 ,0 , Orange.getAngle());
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
        case 110:
            System.out.println("Right and Center");
            OrangeDrive.mecanumDrive_Cartesian(-.5 ,.3 ,0 , Orange.getAngle());
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
        case 011:
            System.out.println("Left and Center");
            OrangeDrive.mecanumDrive_Cartesian(.5,.3 ,0, Orange.getAngle());
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY(),joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
        case 000:
            System.out.println("U DUN GOOFED MATE!");
            if (joyRight.getRawButton(2))
            {
                OrangeDrive.mecanumDrive_Cartesian(joyRight.getX(),joyRight.getY() * -1 ,joyRight.getTwist() * -1 , Orange.getAngle());
            }
            break;
    }
    }

    public void CameraImage() {
     if (camera.freshImage() == false){
            try {
                image = camera.getImage();
                DriverStationLCD.getInstance().updateLCD();
                image.free();

                }
            catch (AxisCameraException ex) {
                    ex.printStackTrace();
                }
            catch (NIVisionException ex) {
                    ex.printStackTrace();
                }
        }
    }

    public void ForkLift() {

        forklift1.set(joyLeft.getY());
        forklift2.set(joyLeft.getY());

    }

    void updateDashboard() {
                 Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 4;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 6;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            // lowDashData.addByte(Solenoid.getAll());
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

     }
    
}
